#include "kcftracker.hpp"
#include <opencv2/opencv.hpp>

#define Max_linear_speed 0.6
#define Min_linear_speed 0.4
#define Min_distance 1.5
#define Max_distance 5.0
#define Max_rotation_speed 0.75

float linear_speed = 0;
float rotation_speed = 0;

float k_linear_speed = (Max_linear_speed - Min_linear_speed) / (Max_distance - Min_distance);
float h_linear_speed = Min_linear_speed - k_linear_speed * Min_distance;

float k_rotation_speed = 0.004;
float h_rotation_speed_left = 1.2;
float h_rotation_speed_right = 1.36;

int ERROR_OFFSET_X_left1 = 100;
int ERROR_OFFSET_X_left2 = 300;
int ERROR_OFFSET_X_right1 = 340;
int ERROR_OFFSET_X_right2 = 540;

bool select_flag = false;
bool bRenewROI = false; // the flag to enable the implementation of KCF algorithm for the new chosen ROI
bool bBeginKCF = false;
bool enable_get_depth = false;

cv::Mat rgbimage;
cv::Mat depthimage;
cv::Rect selectRect;
cv::Point origin;
cv::Rect result;

void onMouse(int event, int x, int y, int, void *)
{
    if (select_flag)
    {
        selectRect.x = MIN(origin.x, x);
        selectRect.y = MIN(origin.y, y);
        selectRect.width = abs(x - origin.x);
        selectRect.height = abs(y - origin.y);
        selectRect &= cv::Rect(0, 0, rgbimage.cols, rgbimage.rows);
    }
    if (event == cv::EVENT_LBUTTONDOWN)
    {
        bBeginKCF = false;
        select_flag = true;
        origin = cv::Point(x, y);
        selectRect = cv::Rect(x, y, 0, 0);
    }
    else if (event == cv::EVENT_LBUTTONUP)
    {
        select_flag = false;
        bRenewROI = true;
    }
}

int main(int argc, char **argv)
{
    (void)argc;
    (void)argv;

    bool HOG = true;
    bool FIXEDWINDOW = false;
    bool MULTISCALE = true;
    bool LAB = false;

    KCFTracker tracker(HOG, FIXEDWINDOW, MULTISCALE, LAB);

    cv::VideoCapture capture(0);
    if (!capture.isOpened())
    {
        std::cout << "camera open fail" << std::endl;
    }
    while (capture.read(rgbimage))
    {
        cv::setMouseCallback("camera", onMouse, 0);

        if (bRenewROI)
        {
            tracker.init(selectRect, rgbimage);
            bBeginKCF = true;
            bRenewROI = false;
            enable_get_depth = false;
        }

        if (bBeginKCF)
        {
            result = tracker.update(rgbimage);
            cv::rectangle(rgbimage, result, cv::Scalar(0, 255, 255), 1, 8);
            enable_get_depth = true;
        }
        else
        {
            cv::rectangle(rgbimage, selectRect, cv::Scalar(255, 0, 0), 2, 8, 0);
        }

        cv::imshow("camera", rgbimage);

        if (cv::waitKey(1) == 27)
        {
            break;
        }
    }
    capture.release();
    cv::destroyAllWindows();

    return 0;
}